// This does not have bounds checking
// Test only with valid input
// (0,0) is top left of graphical representation
// 0=up, 1=right, 2=down, 3=left
// x indices on side, y indices on top
/*
    y0 y1 y2 y3
  x0
  x1
  x2
  x3
*/

#include "math.h"
#include <Wire.h>
#include <Adafruit_MotorShield.h>
#include "utility/Adafruit_PWMServoDriver.h"
#include <HC_SR04.h>

// Creates the motor shield object with the default I2C address
Adafruit_MotorShield AFMS = Adafruit_MotorShield(); 

// Select which 'port' M1, M2, M3 or M4.
Adafruit_DCMotor *myMotor1 = AFMS.getMotor(3);  // M1 Front L 1 3
Adafruit_DCMotor *myMotor2 = AFMS.getMotor(4);  // M2 Front R 2 4
Adafruit_DCMotor *myMotor3 = AFMS.getMotor(1);  // M3 Rear L 3 1
Adafruit_DCMotor *myMotor4 = AFMS.getMotor(2);  // M4 Rear R 4 2

// Creates ultrasonic sensor objects, parameters are pin numbers
HC_SR04 us1(30,31);
HC_SR04 us2(32,33);
HC_SR04 us3(34,35);
HC_SR04 us4(36,37);
HC_SR04 us5(38,39);
HC_SR04 us6(40,41);

int currx, curry, dir, sens_num, sens_dis, a, map_res=18;
int sens_num1, sens_num2, sens_num3, sens_num4, sens_num5, sens_num6;
int sens_dis1, sens_dis2, sens_dis3, sens_dis4, sens_dis5, sens_dis6;
int mymap[18][18];
char junk = ' ';
boolean debug=false; // when true, this will print out variable values for verification
int motorspeed=128;
long cm, inches1, inches2, inches3, inches4, inches5, inches6;


void setup() {
  Serial.begin(9600);
  Serial.flush();
  AFMS.begin(); // create with the default frequency 1.6KHz
  uint8_t i;
  init_map();
}

void loop() {
  // 128 speed: delay(~4100)=4ft
  
  
  // this will move the robot forward in 1 ft increments.
  // the robot will pause, take an US sensor reading,
  // add the wall to the map, then move again.
  
  /*
  // 1
  motors_forward(920);
  get_input();
  get_distance();
  sens_dis = inches/6;
  add_obstacle();
  print_map();
  delay(1000);
  
  // 2
  motors_forward(950);
  get_input();
  get_distance();
  sens_dis = inches/6;
  add_obstacle();
  print_map();
  delay(1000);
  
  // 3
  motors_forward(875);
  get_input();
  get_distance();
  sens_dis = inches/6;
  add_obstacle();
  print_map();
  delay(1000);
  
  // 4
  motors_forward(950);
  get_input();
  get_distance();
  sens_dis = inches/6;
  add_obstacle();
  print_map();
  delay(1000);
  
  turn_left(960);
  delay(1000);
  
  */
  
  get_input();
  
  get_distance_all();
  add_walls();
  print_map();
    
}

void print_distance(int sens_num){
  if(sens_num == 1){
    Serial.print("Inches1: ");
    Serial.println(inches1);
    Serial.print("sens_dis1: ");
    Serial.println(sens_dis1);
  }
  else if(sens_num == 2){
    Serial.print("Inches2: ");
    Serial.println(inches2);
    Serial.print("sens_dis2: ");
    Serial.println(sens_dis2);
  }
  else if(sens_num == 3){
    Serial.print("Inches3: ");
    Serial.println(inches3);
    Serial.print("sens_dis3: ");
    Serial.println(sens_dis3);
  }
  else if(sens_num == 4){
    Serial.print("Inches4: ");
    Serial.println(inches4);
    Serial.print("sens_dis4: ");
    Serial.println(sens_dis4);
  }
  else if(sens_num == 5){
    Serial.print("Inches5: ");
    Serial.println(inches5);
    Serial.print("sens_dis5: ");
    Serial.println(sens_dis5);
  }
  else if(sens_num == 6){
    Serial.print("Inches6: ");
    Serial.println(inches6);
    Serial.print("sens_dis6: ");
    Serial.println(sens_dis6);
  }
}

// add perimeter walls to map
void init_map(){
  for(int i = 0; i < map_res; i++){
    for(int j = 0; j < map_res; j++){
      if(i == 0 || j == 0 || i == map_res-1 || j == map_res-1){
        mymap[i][j] = 1;
      }
    }
  }
}

// ask user for (x,y), theta, sensor info
void get_input(){
  // enter x value
  Serial.println("Enter value for x, Press ENTER");
  while (Serial.available() == 0) ;  // Wait here until input buffer has a character{
  currx = Serial.parseFloat();
  if(debug) Serial.print("currx = "); Serial.println(currx, DEC);
  while (Serial.available() > 0)  // .parseFloat() can leave non-numeric characters
  { junk = Serial.read() ; }      // clear the keyboard buffer
    
  // enter y value:
  Serial.println("Enter value for y, Press ENTER");
  while (Serial.available() == 0) ;
  curry = Serial.parseFloat();
  if(debug) Serial.print("curry = "); Serial.println(curry, DEC);
  while (Serial.available() > 0)
  { junk = Serial.read() ; }
  
  // enter direction (theta)
  Serial.println("Enter value for direction (theta), Press ENTER");
  while (Serial.available() == 0) ;
  dir = Serial.parseFloat();
  if(debug) Serial.print("dir = "); Serial.println(dir, DEC);
  while (Serial.available() > 0)
  { junk = Serial.read() ; }
  
  /*
  // enter sensor number
  Serial.println("Enter sensor number, Press ENTER");
  while (Serial.available() == 0) ;
  sens_num = Serial.parseFloat();
  if(debug) Serial.print("sens_num = "); Serial.println(sens_num, DEC);
  while (Serial.available() > 0)
  { junk = Serial.read() ; }
  
    // enter sensor distance
  Serial.println("Enter sensor distance, Press ENTER");
  while (Serial.available() == 0) ;
  sens_dis = Serial.parseFloat();
  if(debug) Serial.print("sens_dis = "); Serial.println(sens_dis, DEC);
  while (Serial.available() > 0)
  { junk = Serial.read() ; }
  */
}

void add_walls(){
  add_obstacle(1);
  add_obstacle(2);
  //add_obstacle(3);
  add_obstacle(4);
  //add_obstacle(5);
  //add_obstacle(6);
}

// use sensor info to add wall to map
// NOTE: TESTING DIR==0 WITH ALL SIX SENSORS
// IF IT WORKS, MODIFY OTHER 3 IFS TO BE LIKE DIR==0
// FIX SENS_DIS
void add_obstacle(int sens_numb){
  mymap[currx][curry] = 3;
  // robot facing up
  if(dir == 0){
     // determine sensor
     if(sens_numb == 1) mymap[currx-sens_dis1][curry] = 1;
     else if(sens_numb == 2) mymap[currx][curry+sens_dis2] = 1;
     else if(sens_numb == 3) mymap[currx][curry+sens_dis3] = 1;
     else if(sens_numb == 4) mymap[currx+sens_dis4][curry] = 1;
     else if(sens_numb == 5) mymap[currx][curry-sens_dis5] = 1;
     else if(sens_numb == 6) mymap[currx][curry-sens_dis6] = 1;
  }
  
  // robot facing right
  else if(dir == 1){
     // determine sensor
     if(sens_numb == 1) mymap[currx][curry+sens_dis1] = 1;
     else if(sens_numb == 2) mymap[currx+sens_dis2][curry] = 1;
     else if(sens_numb == 3) mymap[currx+sens_dis3][curry] = 1;
     else if(sens_numb == 4) mymap[currx][curry-sens_dis4] = 1;
     else if(sens_numb == 5) mymap[currx-sens_dis5][curry] = 1;
     else if(sens_numb == 6) mymap[currx-sens_dis6][curry] = 1;
  }
  
  // robot facing down
  else if(dir == 2){
     // determine sensor
     if(sens_num == 1) mymap[currx+sens_dis1][curry] = 1;
     else if(sens_num == 2) mymap[currx][curry-sens_dis2] = 1;
     else if(sens_num == 3) mymap[currx][curry-sens_dis3] = 1;
     else if(sens_num == 4) mymap[currx-sens_dis4][curry] = 1;
     else if(sens_num == 5) mymap[currx][curry+sens_dis5] = 1;
     else if(sens_num == 6) mymap[currx][curry+sens_dis6] = 1;
  }
  
  // robot facing left
  else if(dir == 3){
     // determine sensor
     if(sens_num == 1) mymap[currx][curry-sens_dis1] = 1;
     else if(sens_num == 2) mymap[currx-sens_dis2][curry] = 1;
     else if(sens_num == 3) mymap[currx-sens_dis3][curry] = 1;
     else if(sens_num == 4) mymap[currx][curry+sens_dis4] = 1;
     else if(sens_num == 5) mymap[currx+sens_dis5][curry] = 1;
     else if(sens_num == 6) mymap[currx+sens_dis6][curry] = 1;
  }
}

void print_map(){
  Serial.println("3: current position\n2: previous position\n1: wall found\n");
  for(int i = 0; i < map_res; i++){
     for(int j = 0; j < map_res; j++){
        //if(mymap[i][j] != 0) Serial.print(mymap[i][j], DEC);
        if(mymap[i][j] == 1 || mymap[i][j] == 3){
          Serial.print(mymap[i][j], DEC);
        }
        else Serial.print("-");
        Serial.print(" ");
     }
     Serial.println();
  }
  Serial.println();
  mymap[currx][curry] = 2;
}

void set_speed(){
  myMotor1->setSpeed(motorspeed-7);
  myMotor2->setSpeed(motorspeed);
  myMotor3->setSpeed(motorspeed);
  myMotor4->setSpeed(motorspeed-7);
}

void turn_left(int delay_time){
  myMotor1->run(FORWARD);
  myMotor2->run(BACKWARD);
  myMotor3->run(BACKWARD);
  myMotor4->run(FORWARD);
  set_speed();
  delay(delay_time);
  motors_stop();
}

void turn_right(int delay_time){
  myMotor1->run(BACKWARD);
  myMotor2->run(FORWARD);
  myMotor3->run(FORWARD);
  myMotor4->run(BACKWARD);
  set_speed();
  delay(delay_time);
  motors_stop();
}

void motors_forward(int delay_time){
  myMotor1->run(BACKWARD);
  myMotor2->run(BACKWARD);
  myMotor3->run(FORWARD);
  myMotor4->run(FORWARD);
  set_speed();
  delay(delay_time);
  motors_stop();
}

void motors_backward(int delay_time){
  myMotor1->run(FORWARD);
  myMotor2->run(FORWARD);
  myMotor3->run(BACKWARD);
  myMotor4->run(BACKWARD);
  set_speed();
  delay(delay_time);
  motors_stop();
}

void motors_stop(){
  myMotor1->run(RELEASE);
  myMotor2->run(RELEASE);
  myMotor3->run(RELEASE);
  myMotor4->run(RELEASE);
}

void get_distance_all(){
  // this will get the distance measurement from each sensor
  // TODO: ROUND FRACTION TO NEAREST WHOLE NUMBER; DON'T JUST DROP REMAINDER
  
  inches1 = us1.getDistance(false);
  sens_dis1 = inches1 / 6;
  delay(60);
  print_distance(1);
  
  /*inches2 = us2.getDistance(false);
  sens_dis2 = inches2 / 6;
  print_distance(2);
  */
  inches3 = us3.getDistance(false);
  sens_dis3 = inches3 / 6;
  delay(60);
  print_distance(3);
  
  inches4 = us4.getDistance(false);
  sens_dis4 = inches4 / 6;
  delay(60);
  print_distance(4);
  
  inches5 = us5.getDistance(false);
  sens_dis5 = inches5 / 6;
  delay(60);
  print_distance(5);
  
  /*inches6 = us6.getDistance(false);
  sens_dis6 = inches6 / 6;
  print_distance(6);
  */
  delay(60);
}

void get_distance1(){
  inches1 = us1.getDistance(false);
  sens_dis1 = inches1 / 6;
  delay(60);
  //print_distance(1);
}

void get_distance2(){
  inches2 = us2.getDistance(false);
  sens_dis2 = inches2 / 6;
  delay(60);
  //print_distance(2);
}

void get_distance3(){
  inches3 = us3.getDistance(false);
  sens_dis3 = inches3 / 6;
  delay(60);
  print_distance(3);
}

void get_distance4(){
  inches4 = us4.getDistance(false);
  sens_dis4 = inches4 / 6;
  delay(60);
  print_distance(4);
}

void get_distance5(){
  inches5 = us5.getDistance(false);
  sens_dis5 = inches5 / 6;
  delay(60);
  print_distance(5);
}

void get_distance6(){
  inches6 = us6.getDistance(false);
  sens_dis6 = inches6 / 6;
  delay(60);
  print_distance(6);
}
